#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <linux/i2c.h>		// if compile error, please comment.
#include <linux/i2c-dev.h>

/*
 *	case I2C_RDWR:
		return i2cdev_ioctl_rdrw(client, arg);

	case I2C_SMBUS:
		return i2cdev_ioctl_smbus(client, arg);

	i2cdev_ioctl_rdrw  ---> i2c_transfer	---> adap->algo->master_xfer
	i2cdev_ioctl_smbus ---> i2c_smbus_xfer  ---> adap->algo->smbus_xfer

 */

#define VL53L0X_REG_IDENTIFICATION_MODEL_ID         0xc0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID      0xc2
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD   0x50
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define VL53L0X_REG_SYSRANGE_START                  0x00
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS         0x13
#define VL53L0X_REG_RESULT_RANGE_STATUS             0x14

#define SLAVE_ADDR		0x29
#define I2C_DEV			"/dev/i2c-1"

int open_i2c(void);
void close_i2c(int dev_id);
void hex_dump(unsigned char *values, int count);

int read_i2c(int dev_id, int slave_address, int reg_address);
int read_i2c_block_frequency(int dev_id, int slave_address, int reg_address, int frequency, unsigned char * data );
int read_i2c_block(int dev_id, int slave_address, int reg_address, int len, unsigned char * data);
int write_i2c(int dev_id, int slave_address, int reg_address, int value);
int write_i2c_nbytes(int dev_id, int slave_address, int reg_address, unsigned char *val, int len);

int period_pclks(int value);
short makeuint16(int lsb, int msb) ;

unsigned char invoke_read_i2c(int dev_id, int reg_address);
unsigned char invoke_read_i2c_nbyte(int dev_id, int reg_address, unsigned char * data, int len);
void invoke_read_i2c_block(int dev_id, int reg_address, unsigned char * data, int len);
void invoke_write_i2c(int dev_id, unsigned char * data, int len);


int open_i2c(void)
{
	int dev_id;

	dev_id = open( I2C_DEV, O_RDWR );
	if (dev_id < 0) {
		perror("i2c open error.\n");
		exit(1);
	}

	return dev_id;
}

void close_i2c(int dev_id)
{
	close(dev_id);
}

void hex_dump(unsigned char *values, int count)
{
	while( count-- > 0 ){
		printf( "0x%02x ", *(values++) );
	}
	putchar(0xa);
}

/**************************************************************************************************
 * i2c application for driver of struct method
 *
 * 还可以使用I2C_RDWR实现同样的功能
 */
int _main(int argc, char **argv)  
{  
    struct i2c_rdwr_ioctl_data iic_data;  
    unsigned int fd;  
    unsigned int slave_address, reg_address,value;   
    //slave_address为iic的地址，reg_address为iic中存储单元的地址，范围0x0--0xFFFFFFFF,value为你要写进iic的值  
    int ret;  

    if (argc < 5){  
        printf("Usage:\n%s /dev/i2c/x start_addr reg_addr value\n",argv[0]);  
        return 0;  
    }  
      
    fd = open(argv[1], O_RDWR);  
      
    if (!fd){  
        printf("Error on opening the device file\n");  
        return 0;  
    }  
  
    sscanf(argv[2], "%x", &slave_address);  
    sscanf(argv[3], "%x", &reg_address);  
    sscanf(argv[4], "%x", &value);  
      
    iic_data.nmsgs = 2;//因为都时序要两次，所以设为2  
    iic_data.msgs = (struct i2c_msg *)malloc(iic_data.nmsgs * sizeof(struct i2c_msg));  
    if (!iic_data.msgs){  
        printf("Memory alloc error\n");  
        close(fd);  
        return 0;  
    }  
      
    ioctl(fd, I2C_TIMEOUT, 2);//设置超时时间  
    ioctl(fd, I2C_RETRIES, 1);//设置重发次数  

    /* 
	 * write data to iic 
	 */
    iic_data.nmsgs = 1;  
    iic_data.msgs[0].len = 2;				// 信息长度为2，看写时序，iic的地址不算的，因为付给了addr，而len是指buf中的值的个数  
    iic_data.msgs[0].addr = slave_address;  
    iic_data.msgs[0].flags = 0;				// 写命令  
    iic_data.msgs[0].buf = (unsigned char*)malloc(2);  
    iic_data.msgs[0].buf[0] = reg_address;	// 信息值1 iic中存储单元的地址，即你要往哪写  
    iic_data.msgs[0].buf[1] = value;			// 信息值2,即你要写什么  
      
    ret = ioctl (fd, I2C_RDWR, (unsigned long)&iic_data);// 好了 ，写进去吧  
    if (ret < 0){  
        printf ("ioctl write error\n");  
    }  
  
    printf("you have write %02x into iic at %02x address\n",value,reg_address);  
      
    sleep(1);  

    /*
	 * read data from iic
	 */  
    iic_data.nmsgs = 2;						// 读时序要两次过程，要发两次I2C消息  
    iic_data.msgs[0].len = 1;				// 信息长度为1，第一次只写要读的iic中存储单元的地址 
    iic_data.msgs[0].addr = slave_address;  
    iic_data.msgs[0].flags = 0;				// 写命令，看读时序理解 
	iic_data.msgs[0].buf = (unsigned char*)malloc(1); 
    iic_data.msgs[0].buf[0] = reg_address;	// 信息值  
          
    iic_data.msgs[1].len = 1;  
    iic_data.msgs[1].addr = slave_address;  
    iic_data.msgs[1].flags = I2C_M_RD;		// 读命令  
    iic_data.msgs[1].buf = (unsigned char*)malloc(1);  
    iic_data.msgs[1].buf[0] = 0;				// 先清空要读的缓冲区  

    ret = ioctl (fd, I2C_RDWR, (unsigned long)&iic_data);// 好了，读吧  
    if (ret < 0) {  
        printf ("ioctl read error\n");  
    }  
       
    printf("read %02x from iic address %02x\n",iic_data.msgs[1].buf[0], reg_address);  
      
	free(iic_data.msgs);
    close(fd);  
    return 0;     
}  

int read_i2c(int dev_id, int slave_address, int reg_address)
{
	struct i2c_rdwr_ioctl_data iic_data;  
	int data = -1;
	int ret  = -1;

	iic_data.nmsgs = 2; 
    iic_data.msgs = (struct i2c_msg *)malloc(iic_data.nmsgs * sizeof(struct i2c_msg));  
    if (!iic_data.msgs){  
        printf("Memory alloc error\n");  
		free(iic_data.msgs);
        close(dev_id);  
        return -1;  
    }

	iic_data.nmsgs = 2;						// 读时序要两次过程，要发两次I2C消息  
    iic_data.msgs[0].len = 1;				// 信息长度为1，第一次只写要读的iic中存储单元的地址 
    iic_data.msgs[0].addr = slave_address;  
    iic_data.msgs[0].flags = 0;				// 写命令，看读时序理解 
	iic_data.msgs[0].buf = (unsigned char*)malloc(1); 
    iic_data.msgs[0].buf[0] = reg_address;	// 信息值  
          
    iic_data.msgs[1].len = 1;  
    iic_data.msgs[1].addr = slave_address;  
    iic_data.msgs[1].flags = I2C_M_RD;		// 读命令  
    iic_data.msgs[1].buf = (unsigned char*)malloc(1);  
    iic_data.msgs[1].buf[0] = 0;				// 先清空要读的缓冲区  

    ret = ioctl (dev_id, I2C_RDWR, (unsigned long)&iic_data);  
    if (ret < 0) {  
        printf ("ioctl read error\n"); 
		free(iic_data.msgs[0].buf);
		free(iic_data.msgs);
		return -1;
    }  
       
    //printf("read %02x from iic address %02x\n",iic_data.msgs[1].buf[0], reg_address);  
	data = iic_data.msgs[1].buf[0];

	free(iic_data.msgs[0].buf);
	free(iic_data.msgs);

	return data;
}

int read_i2c_block_frequency(int dev_id, int slave_address, int reg_address, int frequency, unsigned char * data )
{
	int i;

	for (i = 0; i < frequency; i++) {
		data[i] = read_i2c(dev_id, slave_address, reg_address + i);
		// printf("data[%d] = %d = 0x%x\n", i, data[i], data[i]);
		sleep(0.5);
	}

	hex_dump(data, frequency);

	return 0;
}

int read_i2c_block(int dev_id, int slave_address, int reg_address, int len, unsigned char * data)
{
	struct i2c_rdwr_ioctl_data iic_data;  
	//int data = -1;
	int ret  = -1;

	iic_data.nmsgs = 2; 
    iic_data.msgs = (struct i2c_msg *)malloc(iic_data.nmsgs * sizeof(struct i2c_msg));  
    if (!iic_data.msgs) {  
        printf("Memory alloc error\n");  
		free(iic_data.msgs);
        close(dev_id);  
        return -1;  
    }

	iic_data.nmsgs = 2;						// 读时序要两次过程，要发两次I2C消息  
    iic_data.msgs[0].len = 1;				// 信息长度为1，第一次只写要读的iic中存储单元的地址 
    iic_data.msgs[0].addr = slave_address;  
    iic_data.msgs[0].flags = 0;				// 写命令，看读时序理解 
	iic_data.msgs[0].buf = (unsigned char*)malloc(1); 
    iic_data.msgs[0].buf[0] = reg_address;	// 信息值  
          
    iic_data.msgs[1].len = len;  
    iic_data.msgs[1].addr = slave_address;  
    iic_data.msgs[1].flags = I2C_M_RD;		// 读命令  
    iic_data.msgs[1].buf = data;  
    //iic_data.msgs[1].buf[0] = 0;				// 先清空要读的缓冲区  

    ret = ioctl (dev_id, I2C_RDWR, (unsigned long)&iic_data);  
    if (ret < 0) {  
        printf ("ioctl read error\n"); 
		free(iic_data.msgs[0].buf);
		free(iic_data.msgs);
		return -1;
    }  
       
    //printf("read %02x from iic address %02x\n",iic_data.msgs[1].buf[0], reg_address);  
	//data = iic_data.msgs[1].buf[0];
	hex_dump(data, len);

	free(iic_data.msgs[0].buf);
	free(iic_data.msgs);

	return 0;
}

int write_i2c(int dev_id, int slave_address, int reg_address, int value)
{
	struct i2c_rdwr_ioctl_data iic_data;
	int ret  = -1;

    iic_data.nmsgs = 2;
    iic_data.msgs = (struct i2c_msg *)malloc(iic_data.nmsgs * sizeof(struct i2c_msg));  
    if (!iic_data.msgs){  
        printf("Memory alloc error\n");  
        close(dev_id);  
		free(iic_data.msgs);
        return -1;  
    } 

	ioctl(dev_id, I2C_TIMEOUT, 2);//设置超时时间  
    ioctl(dev_id, I2C_RETRIES, 1);//设置重发次数  
	
	iic_data.nmsgs = 1;  
    iic_data.msgs[0].len = 2;				// 信息长度为2，看写时序，iic的地址不算的，因为付给了addr，而len是指buf中的值的个数  
    iic_data.msgs[0].addr = slave_address;  
    iic_data.msgs[0].flags = 0;				// 写命令  
    iic_data.msgs[0].buf = (unsigned char*)malloc(2);  
    iic_data.msgs[0].buf[0] = reg_address;	// 信息值1 iic中存储单元的地址，即你要往哪写  
    iic_data.msgs[0].buf[1] = value;			// 信息值2,即你要写什么  
      
    ret = ioctl (dev_id, I2C_RDWR, (unsigned long)&iic_data);
    if (ret < 0) {  
        printf ("ioctl write error\n");
		free(iic_data.msgs[0].buf);
		free(iic_data.msgs);
		return -1;
    }  
  
    printf("you have write %02x into iic at %02x address\n",value,reg_address); 
	
	free(iic_data.msgs[0].buf);
	free(iic_data.msgs);
	
	return 0;
}

int write_i2c_nbytes(int dev_id, int slave_address, int reg_address, unsigned char *val, int len)
{
    int ret = 0;
    struct i2c_rdwr_ioctl_data packets;
    struct i2c_msg messages;
    int i;

    packets.nmsgs = 1;
    packets.msgs = &messages;

    //发送要读取的寄存器地址
    messages.addr = slave_address;
    messages.flags = 0;         //write
    messages.len = len + 1;     //数据长度
    //发送数据
    messages.buf = (unsigned char *)malloc(len+1);
    if (NULL == messages.buf)
    {
        ret = -1;
        goto err;
    }

    messages.buf[0] = reg_address;
    for (i = 0; i < len; i++)
    {
        messages.buf[1+i] = val[i];
    }

    ret = ioctl(dev_id, I2C_RDWR, (unsigned long)&packets);
    if (ret < 0){
        printf("write error!\n");
        return -1;
    }

err:
    free(messages.buf);

    return ret;
}
/*************************************************************************************************/


int period_pclks(int value)
{
	return ((value + 1) << 1);
}

short makeuint16(int lsb, int msb) 
{
    return ((msb & 0xFF) << 8) | (lsb & 0xFF);
}


/**************************************************************************************************
 *	i2c application for read()/write() method
 *
 * 使用操作普通文件的接口read()和write()。这两个函数间接调用了i2c_master_recv和i2c_master_send。
 * 但是在使用之前需要使用I2C_SLAVE设置从机地址，设置可能失败，需要检查返回值。
 * 这种通信过程进行I2C层的通信，一次只能进行一个方向的传输。
 */
unsigned char invoke_read_i2c(int dev_id, int reg_address)
{
	unsigned char value = 0xff;

	write(dev_id, &reg_address, 1);		// write reg addr for first
	read(dev_id, &value,1);				// read a value

	return value;
}

unsigned char invoke_read_i2c_nbyte(int dev_id, int reg_address, unsigned char * data, int len)
{
	write(dev_id, &reg_address, 1);		// write reg addr for first
	read(dev_id, data,len);				// read n many values

	return 0;
}

void invoke_read_i2c_block(int dev_id, int reg_address, unsigned char * data, int len)
{
	int i;

	write(dev_id, &reg_address, 1);
	for (i = 0; i < len; i++) {
		data[i] = invoke_read_i2c(dev_id, reg_address+i);
	}
}

void invoke_write_i2c(int dev_id, unsigned char * data, int len)
{
	write(dev_id, data, len);			// write reg addr and value
}

/*************************************************************************************************/

int pp_main()
{
	int dev;
	unsigned char data[16] = {0};
	int ret = -2;
	int count = 0;	
	unsigned short acnt, scnt, dist;
	unsigned char status;

	dev = open_i2c();

	/* 7bit addrs  */
	if (ioctl(dev, I2C_TENBIT, 0) != 0) {
		perror("I2C_TENBIT set was fail.\n");
		return -1;
	}

	/* i2c slave address */
	if (ioctl(dev, I2C_SLAVE, SLAVE_ADDR) != 0) {
		perror("I2C_SLAVE set was fail.\n");
		return -1;
	}


	ret = invoke_read_i2c(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID);
	printf("Device ID = 0x%x\n", ret);

	ret = invoke_read_i2c(dev, VL53L0X_REG_IDENTIFICATION_REVISION_ID);
	printf("Revision ID = 0x%x\n", ret);

	ret = invoke_read_i2c(dev, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD);
	printf("pre range config vcsel period = 0x%x\n", period_pclks(ret));

	ret = invoke_read_i2c(dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD);
	printf("final range config vcsel period = 0x%x\n", period_pclks(ret));

	// read n byte of test
	memset(data, 0, sizeof(data));
	invoke_read_i2c_nbyte(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, data,3);
	hex_dump( data, 3 );	// 0xee 0xaa 0x10

	
	// write a byte of test
	memset(data, 0, sizeof(data));
	data[0] = VL53L0X_REG_SYSRANGE_START;
	data[1] = 0x09;
	invoke_write_i2c(dev, data, 2);
	
	ret = invoke_read_i2c(dev, VL53L0X_REG_SYSRANGE_START);
	printf("SYSRANGE_START = 0x%x\n", ret);		

	
	// write n byte of test
	memset(data, 0, sizeof(data));
	data[0] = VL53L0X_REG_SYSRANGE_START;
	data[1] = 0x0a;
	data[2] = 0x0a;
	data[3] = 0x0a;
	invoke_write_i2c(dev, data, 4);

	memset(data, 0, sizeof(data));
	invoke_read_i2c_nbyte(dev, VL53L0X_REG_SYSRANGE_START, data,3);
	hex_dump( data, 3 );


	while (1) {
		memset(data, 0, sizeof(data));
		data[0] = VL53L0X_REG_SYSRANGE_START;
		data[1] = 0x01;
		invoke_write_i2c(dev, data, 2);

		count = 0;
		while (count < 100) {
			sleep(0.05);
			ret = invoke_read_i2c(dev, VL53L0X_REG_RESULT_RANGE_STATUS);

			if (ret & 0x01)
				break;

			count++;
		}

		if (ret & 0x01)
			printf("ready\n");
		else {
			printf("not ready\n");
			continue;
			exit(1);
		}
	
		memset(data, 0, sizeof(data));
		//invoke_read_i2c_nbyte(dev, VL53L0X_REG_RESULT_RANGE_STATUS, data, 12);
		invoke_read_i2c_block(dev, VL53L0X_REG_RESULT_RANGE_STATUS, data, 12);
		hex_dump(data, 12);

		acnt = makeuint16(data[7], data[6]);
		scnt = makeuint16(data[9], data[8]);
		dist = makeuint16(data[11], data[10]);
		status = ((data[0] & 0x78) >> 3);

		printf("ambient count: %d\n", acnt);
		printf("signal count: %d\n", scnt);
		printf("distance: %d\n", dist);
		printf("status: %d\n",status);
		sleep(1);
	}

	close(dev);
	return 0;
}


int main()
{
	int dev;
	unsigned char data[16] = {0};
	int ret = -2;
	int count = 0;	
	short acnt, scnt, dist;
	unsigned char status;

	dev = open_i2c();

	/* 7bit addrs  */
	if (ioctl(dev, I2C_TENBIT, 0) != 0) {
		perror("I2C_TENBIT set was fail.\n");
		return -1;
	}

	/* i2c slave address */
	if (ioctl(dev, I2C_SLAVE, SLAVE_ADDR) != 0) {
		perror("I2C_SLAVE set was fail.\n");
		return -1;
	}

	ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_IDENTIFICATION_MODEL_ID);
	printf("Device ID = 0x%x\n", ret);

	//ret = read_i2c(dev, 0x29, 0xc1);
	//printf("C1 = 0x%x\n", ret);

	ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_IDENTIFICATION_REVISION_ID);
	printf("Revision ID = 0x%x\n", ret);

	ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD);
	printf("pre range config vcsel period = 0x%x\n", period_pclks(ret));

	ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD);
	printf("final range config vcsel period = 0x%x\n", period_pclks(ret));

	ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_SYSRANGE_START);
	printf("start = 0x%x\n", ret);

	ret = write_i2c(dev, SLAVE_ADDR, VL53L0X_REG_SYSRANGE_START, 0x01);
	printf("ret = 0x%x\n", ret);


	ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_SYSRANGE_START);
	printf("read_i2c  = 0x%x\n", ret);

	ret = write_i2c(dev, SLAVE_ADDR, VL53L0X_REG_SYSRANGE_START, 0x09);
	printf("write_i2c ret = 0x%x\n", ret);

	ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_SYSRANGE_START);
	printf("read_i2c start = 0x%x\n", ret);

	while (1) {
		write_i2c(dev, SLAVE_ADDR, VL53L0X_REG_SYSRANGE_START, 0x01);	

		count = 0;
		while (count < 100) {
			sleep(0.05);
			ret = read_i2c(dev, SLAVE_ADDR, VL53L0X_REG_RESULT_RANGE_STATUS);

			if (ret & 0x01)
				break;

			count++;
		}

		if (ret & 0x01)
			printf("ready\n");
		else {
			printf("not ready\n");
			continue;
			exit(1);
		}
	
		memset(data, 0, sizeof(data));
		//read_i2c_block(dev, SLAVE_ADDR, VL53L0X_REG_RESULT_RANGE_STATUS, 12, data);
		read_i2c_block_frequency(dev, SLAVE_ADDR, VL53L0X_REG_RESULT_RANGE_STATUS, 12, data);

		acnt = makeuint16(data[7], data[6]);
		scnt = makeuint16(data[9], data[8]);
		dist = makeuint16(data[11], data[10]);
		status = ((data[0] & 0x78) >> 3);

		printf("ambient count: %d\n", acnt);
		printf("signal count: %d\n", scnt);
		printf("distance: %d\n", dist);
		printf("status: %d\n",status);
		sleep(1);
	}

	close(dev);
	return 0;
}



